package Rangel.common.world.tools;

import adf.agent.info.AgentInfo;
import adf.agent.info.WorldInfo;
import com.vividsolutions.jts.algorithm.ConvexHull;
import com.vividsolutions.jts.geom.Coordinate;
import com.vividsolutions.jts.geom.GeometryFactory;
import math.geom2d.line.LineSegment2D;
import rescuecore2.misc.Pair;
import rescuecore2.misc.geometry.GeometryTools2D;
import rescuecore2.misc.geometry.Vector2D;
import rescuecore2.standard.entities.*;
import rescuecore2.worldmodel.EntityID;

import java.awt.*;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.*;
import java.util.List;


public class Util {

    //判断是否相交
    public static boolean intersects(rescuecore2.misc.geometry.Line2D lineSegment1, rescuecore2.misc.geometry.Line2D lineSegment2) {
        lineSegment1.getIntersection(lineSegment2);
        LineSegment2D line1 = new LineSegment2D(lineSegment1.getOrigin().getX(), lineSegment1.getOrigin().getY(), lineSegment1.getEndPoint().getX(), lineSegment1.getEndPoint().getY());
        LineSegment2D line2 = new LineSegment2D(lineSegment2.getOrigin().getX(), lineSegment2.getOrigin().getY(), lineSegment2.getEndPoint().getX(), lineSegment2.getEndPoint().getY());
        return intersects(line1, line2);
    }
    //对线段进行比较
    public static boolean intersects(LineSegment2D lineSegment1, LineSegment2D lineSegment2) {
        return LineSegment2D.intersects(lineSegment1, lineSegment2);
    }


    public static boolean isOnBlockade(WorldInfo worldInfo, AgentInfo agentInfo) {
        if (agentInfo.me() instanceof Human) {
            return isOnBlockade(worldInfo, agentInfo, (Human) agentInfo.me());
        } else {
            return false;
        }
    }

    public static double distance(Polygon polygon, Pair<Integer, Integer> location) {
        return distance(polygon, new rescuecore2.misc.geometry.Point2D(location.first(), location.second()));
    }

    public static boolean isOnBlockade(WorldInfo worldInfo, AgentInfo agentInfo, Human human) {
        if (human != agentInfo.me() && !human.isPositionDefined()) {
            return false;
        }
        StandardEntity se = worldInfo.getEntity(human.getPosition());
        if (se instanceof Road) {
            Blockade blockade;
            Road road = (Road) se;
            if (road.isBlockadesDefined()) {
                for (EntityID id : road.getBlockades()) {
                    blockade = (Blockade) worldInfo.getEntity(id);
                    if (blockade != null && blockade.isApexesDefined()) {
                        if (blockade.getShape().contains(human.getX(), human.getY())) {
                            return true;
                        }
                    }
                }
            }
        }
        return false;

    }

    public static int distance(int x1, int y1, int x2, int y2) {
        float dx = x1 - x2;
        float dy = y1 - y2;
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static double distance(double x1, double y1, double x2, double y2) {
        double dx = x1 - x2;
        double dy = y1 - y2;
        return Math.sqrt(dx * dx + dy * dy);
    }

    public static int distance(Point p1, Point p2) {
        double dx = p1.getX() - p2.getX();
        double dy = p1.getY() - p2.getY();
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static int distance(Point2D p1, Point2D p2) {
        double dx = p1.getX() - p2.getX();
        double dy = p1.getY() - p2.getY();
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static int distance(rescuecore2.misc.geometry.Point2D start, rescuecore2.misc.geometry.Point2D end) {
        double dx = start.getX() - end.getX();
        double dy = start.getY() - end.getY();
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static int distance(Point point, Pair<Integer, Integer> pair) {
        double dx = point.getX() - pair.first();
        double dy = point.getY() - pair.second();
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static int distance(Pair<Integer, Integer> pair, Point point) {
        double dx = point.getX() - pair.first();
        double dy = point.getY() - pair.second();
        return (int) Math.sqrt(dx * dx + dy * dy);
    }

    public static int max(int x, int y) {
        return Math.max(x, y);
    }

    public static int min(int x, int y) {
        return Math.min(x, y);
    }

    public static int distance(Area obj1, Area obj2) {
        return distance(obj1.getX(), obj1.getY(), obj2.getX(), obj2.getY());
    }

    public static int distance(Pair<Integer, Integer> obj1, Pair<Integer, Integer> obj2) {
        return distance(obj1.first(), obj1.second(), obj2.first(), obj2.second());
    }


    public static List<rescuecore2.misc.geometry.Point2D> getPoint2DList(int[] xs, int[] ys) {

        List<rescuecore2.misc.geometry.Point2D> points = new ArrayList<rescuecore2.misc.geometry.Point2D>();
        for (int i = 0; i < xs.length; i++) {
            points.add(new rescuecore2.misc.geometry.Point2D(xs[i], ys[i]));
        }

        return points;
    }

    public static rescuecore2.misc.geometry.Point2D getIntersection(final rescuecore2.misc.geometry.Line2D line1, final rescuecore2.misc.geometry.Line2D line2) {

        final double x1, y1, x2, y2, x3, y3, x4, y4;
        x1 = line1.getOrigin().getX();
        y1 = line1.getOrigin().getY();
        x2 = line1.getEndPoint().getX();
        y2 = line1.getEndPoint().getY();
        x3 = line2.getOrigin().getX();
        y3 = line2.getOrigin().getY();
        x4 = line2.getEndPoint().getX();
        y4 = line2.getEndPoint().getY();
        double v = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
        final double x = ((x2 - x1) * (x3 * y4 - x4 * y3) - (x4 - x3) * (x1 * y2 - x2 * y1))
                / v;
        final double y = ((y3 - y4) * (x1 * y2 - x2 * y1) - (y1 - y2) * (x3 * y4 - x4 * y3))
                / v;

        return new rescuecore2.misc.geometry.Point2D(x, y);

    }

    public static int findDistanceTo(Blockade b, int x, int y) {
        if (b.getShape().contains(x, y)) {
            return 0;
        }
        List<rescuecore2.misc.geometry.Line2D> lines = GeometryTools2D.pointsToLines(GeometryTools2D.vertexArrayToPoints(b.getApexes()), true);
        double best = Double.MAX_VALUE;
        rescuecore2.misc.geometry.Point2D origin = new rescuecore2.misc.geometry.Point2D(x, y);
        for (rescuecore2.misc.geometry.Line2D next : lines) {
            rescuecore2.misc.geometry.Point2D closest = GeometryTools2D.getClosestPointOnSegment(next, origin);
            double d = GeometryTools2D.getDistance(origin, closest);
            if (d < best) {
                best = d;
            }

        }
        return (int) best;
    }

    public static Polygon getPolygon(int[] apexes) {
        Polygon polygon = new Polygon();
        for (int i = 0; i < apexes.length; i += 2) {
            polygon.addPoint(apexes[i], apexes[i + 1]);
        }
        return polygon;
    }


    public static rescuecore2.misc.geometry.Point2D getMiddle(rescuecore2.misc.geometry.Point2D start, rescuecore2.misc.geometry.Point2D end) {

        double cx = ((start.getX() + end.getX()) / 2);
        double cy = ((start.getY() + end.getY()) / 2);

        return new rescuecore2.misc.geometry.Point2D(cx, cy);

    }

    public static rescuecore2.misc.geometry.Point2D getMiddle(rescuecore2.misc.geometry.Line2D line) {
        return getMiddle(line.getOrigin(), line.getEndPoint());
    }

    public static rescuecore2.misc.geometry.Point2D closestPoint(rescuecore2.misc.geometry.Line2D line, rescuecore2.misc.geometry.Point2D point) {
        return GeometryTools2D.getClosestPoint(line, point);
    }

    public static double slope(rescuecore2.misc.geometry.Line2D line) {
        double x1 = line.getOrigin().getX();
        double y1 = line.getOrigin().getY();
        double x2 = line.getEndPoint().getX();
        double y2 = line.getEndPoint().getY();
        return ((y1 - y2) / (x1 - x2));
    }


    public static double lineLength(rescuecore2.misc.geometry.Line2D line) {
        return GeometryTools2D.getDistance(line.getOrigin(), line.getEndPoint());
    }

    public static boolean containsEach(Collection collection1, Collection collection2) {
        for (Object object : collection1) {
            if (collection2.contains(object)) {
                return true;
            }
        }
        return false;
    }

    public static boolean contains(rescuecore2.misc.geometry.Line2D line2D, rescuecore2.misc.geometry.Point2D point) {
        return contains(line2D, point, 0);
    }

    public static boolean contains(rescuecore2.misc.geometry.Line2D line2D, rescuecore2.misc.geometry.Point2D point, double threshold) {
        return !(line2D == null || point == null) && Line2D.ptSegDist(line2D.getOrigin().getX(), line2D.getOrigin().getY(), line2D.getEndPoint().getX(), line2D.getEndPoint().getY(), point.getX(), point.getY()) <= threshold;
    }

    public static double distance(rescuecore2.misc.geometry.Line2D line1, rescuecore2.misc.geometry.Line2D line2) {
        if (Line2D.linesIntersect(line1.getOrigin().getX(), line1.getOrigin().getY(), line1.getEndPoint().getX(), line1.getEndPoint().getY(),
                line2.getOrigin().getX(), line2.getOrigin().getY(), line2.getEndPoint().getX(), line2.getEndPoint().getY())) {
            return 0d;
        }
        double dist1 = distance(line1, line2.getOrigin());
        double dist2 = distance(line1, line2.getEndPoint());
        double dist3 = distance(line2, line1.getOrigin());
        double dist4 = distance(line2, line1.getEndPoint());
        double min = Math.min(dist1, dist2);
        min = Math.min(min, dist3);
        min = Math.min(min, dist4);
        return min;
    }

    public static double distance(rescuecore2.misc.geometry.Line2D line, Polygon polygon) {
        List<rescuecore2.misc.geometry.Line2D> polyLines = getLines(polygon);
        double minDist = Double.MAX_VALUE;
        if (line.getEndPoint().equals(line.getOrigin())) {
            return distance(polygon, line.getOrigin(), true);
        }
        for (rescuecore2.misc.geometry.Line2D polyLine : polyLines) {
            minDist = Math.min(minDist, distance(line, polyLine));
            if (minDist == 0.0) {
                break;
            }
        }
        return minDist;
    }

    public static boolean isPassable(Polygon polygon, Polygon polygon1, int agentPassingThreshold) {

        int count = polygon1.npoints;
        int j;
        double tempDistance;
        boolean isPassable = false;
        for (int i = 0; i < count; i++) {
            j = (i + 1) % count;
            rescuecore2.misc.geometry.Point2D startPoint = new rescuecore2.misc.geometry.Point2D(polygon1.xpoints[i], polygon1.ypoints[i]);
            rescuecore2.misc.geometry.Point2D endPoint = new rescuecore2.misc.geometry.Point2D(polygon1.xpoints[j], polygon1.ypoints[j]);
            if (startPoint.equals(endPoint)) {
                continue;
            }
            rescuecore2.misc.geometry.Line2D poly2Line = new rescuecore2.misc.geometry.Line2D(startPoint, endPoint);
            tempDistance = Util.distance(poly2Line, polygon);
            if (tempDistance < agentPassingThreshold) {
                isPassable = true;
                break;
            }
        }
        return isPassable;

    }

    public static boolean isPassable(Polygon polygon, rescuecore2.misc.geometry.Line2D guideLine, int agentPassingThreshold) {
        boolean isPassable = false;
        double tempDistance = Util.distance(guideLine, polygon);
        if (tempDistance > agentPassingThreshold) {
            isPassable = true;
        }
        return isPassable;
    }


    public static double distance(Polygon polygon1, Polygon polygon2) {
        int count = polygon2.npoints;
        double minDistance = Double.MAX_VALUE;
        int j;
        double distance;
        for (int i = 0; i < count; i++) {
            j = (i + 1) % count;
            rescuecore2.misc.geometry.Point2D startPoint = new rescuecore2.misc.geometry.Point2D(polygon2.xpoints[i], polygon2.ypoints[i]);
            rescuecore2.misc.geometry.Point2D endPoint = new rescuecore2.misc.geometry.Point2D(polygon2.xpoints[j], polygon2.ypoints[j]);
            rescuecore2.misc.geometry.Line2D poly2Line = new rescuecore2.misc.geometry.Line2D(startPoint, endPoint);
            distance = distance(poly2Line, polygon1);
            minDistance = Math.min(minDistance, distance);
            if (minDistance == 0.0) {
                break;
            }
        }
        return minDistance;
    }

    public static double distance(rescuecore2.misc.geometry.Line2D line, rescuecore2.misc.geometry.Point2D point) {
        return Line2D.ptSegDist(line.getOrigin().getX(), line.getOrigin().getY(), line.getEndPoint().getX(), line.getEndPoint().getY(), point.getX(), point.getY());
    }

    public static double distance(rescuecore2.misc.geometry.Line2D line, Pair<Integer, Integer> pair) {
        return Line2D.ptSegDist(line.getOrigin().getX(), line.getOrigin().getY(), line.getEndPoint().getX(), line.getEndPoint().getY(), pair.first().doubleValue(), pair.second().doubleValue());
    }

    public static double distance(Polygon polygon, rescuecore2.misc.geometry.Point2D point) {
        if (polygon.contains(point.getX(), point.getY())) {
            return 0;
        }
        int count = polygon.npoints;
        double minDistance = Double.MAX_VALUE;
        for (int i = 0; i < count; i++) {
            int j = (i + 1) % count;
            rescuecore2.misc.geometry.Point2D stPoint = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[i], polygon.ypoints[i]);
            rescuecore2.misc.geometry.Point2D endPoint = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[j], polygon.ypoints[j]);
            rescuecore2.misc.geometry.Line2D poly2Line = new rescuecore2.misc.geometry.Line2D(stPoint, endPoint);
            double distance = distance(poly2Line, point);
            minDistance = Math.min(minDistance, distance);
            if (minDistance == 0.0) {
                break;
            }
        }
        return minDistance;
    }

    public static double distance(Polygon polygon, rescuecore2.misc.geometry.Point2D point, boolean ignoreInside) {
        if (ignoreInside && polygon.contains(point.getX(), point.getY())) {
            return 0;
        }
        int count = polygon.npoints;
        double minDistance = Double.MAX_VALUE;
        for (int i = 0; i < count; i++) {
            int j = (i + 1) % count;
            rescuecore2.misc.geometry.Point2D stPoint = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[i], polygon.ypoints[i]);
            rescuecore2.misc.geometry.Point2D endPoint = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[j], polygon.ypoints[j]);
            rescuecore2.misc.geometry.Line2D poly2Line = new rescuecore2.misc.geometry.Line2D(stPoint, endPoint);
            double distance = distance(poly2Line, point);
            minDistance = Math.min(minDistance, distance);
            if (minDistance == 0.0) {
                break;
            }
        }
        return minDistance;
    }

    public static boolean isInside(double pX, double pY, int[] apex) {
        rescuecore2.misc.geometry.Point2D p = new rescuecore2.misc.geometry.Point2D(pX, pY);
        Vector2D v1 = (new rescuecore2.misc.geometry.Point2D(apex[apex.length - 2], apex[apex.length - 1])).minus(p);
        Vector2D v2 = (new rescuecore2.misc.geometry.Point2D(apex[0], apex[1])).minus(p);
        double theta = getAngle(v1, v2);

        for (int i = 0; i < apex.length - 2; i += 2) {
            v1 = (new rescuecore2.misc.geometry.Point2D(apex[i], apex[i + 1])).minus(p);
            v2 = (new rescuecore2.misc.geometry.Point2D(apex[i + 2], apex[i + 3])).minus(p);
            theta += getAngle(v1, v2);
        }
        return Math.round(Math.abs((theta / 2) / Math.PI)) >= 1;
    }

    public static double getAngle(Vector2D v1, Vector2D v2) {
        double flag = (v1.getX() * v2.getY()) - (v1.getY() * v2.getX());
        double angle = Math.acos(((v1.getX() * v2.getX()) + (v1.getY() * v2.getY())) / (v1.getLength() * v2.getLength()));
        if (flag > 0) {
            return angle;
        }
        if (flag < 0) {
            return -1 * angle;
        }
        return 0.0D;
    }

    public static double angleBetween2Lines(rescuecore2.misc.geometry.Line2D line1, rescuecore2.misc.geometry.Line2D line2) {
        double theta = Math.acos(line1.getDirection().dot(line2.getDirection()) / (Util.lineLength(line1) * Util.lineLength(line2)));
        return Math.toDegrees(theta);
    }

    public static Point getPointInPolygon(Polygon polygon) {

        int index;
        double cx = polygon.getBounds().getCenterX();
        double cy = polygon.getBounds().getCenterY();
        Point cp = new Point((int) cx, (int) cy);
        if (polygon.contains(cp)) {

            return cp;
        }
        if (polygon.npoints >= 3) {
            index = 2;
        } else {
            return null;
        }
        Point p1 = new Point(polygon.xpoints[0], polygon.ypoints[0]);
        Point center;
        Point p2;
        do {
            p2 = new Point(polygon.xpoints[index], polygon.ypoints[index]);
            center = new Point((int) (p1.getX() + p2.getX()) / 2, (int) (p1.getY() + p2.getY()) / 2);
            index++;
        } while (index < polygon.npoints && !polygon.contains(center));

        return center;
    }

    public static List<rescuecore2.misc.geometry.Point2D> intersections(Polygon polygon, rescuecore2.misc.geometry.Line2D line) {
        List<rescuecore2.misc.geometry.Point2D> intersections = new ArrayList<rescuecore2.misc.geometry.Point2D>();
        List<rescuecore2.misc.geometry.Line2D> polyLines = getLines(polygon);

        for (rescuecore2.misc.geometry.Line2D ln : polyLines) {
            rescuecore2.misc.geometry.Point2D intersectPoint = GeometryTools2D.getSegmentIntersectionPoint(line, ln);
            if (intersectPoint != null) {
                intersections.add(intersectPoint);
            }
        }
        return intersections;
    }

    public static boolean hasIntersection(Polygon polygon, rescuecore2.misc.geometry.Line2D line) {
        List<rescuecore2.misc.geometry.Line2D> polyLines = getLines(polygon);
        if (polygon.contains(line.getOrigin().getX(), line.getOrigin().getY()) ||
                polygon.contains(line.getEndPoint().getX(), line.getEndPoint().getY())) {
            return true;
        }
        for (rescuecore2.misc.geometry.Line2D ln : polyLines) {
            rescuecore2.misc.geometry.Point2D intersectPoint = GeometryTools2D.getSegmentIntersectionPoint(line, ln);
            if (intersectPoint != null) {
                return true;
            }
        }
        return false;
    }

    public static List<rescuecore2.misc.geometry.Line2D> getLines(Polygon polygon) {
        List<rescuecore2.misc.geometry.Line2D> lines = new ArrayList<rescuecore2.misc.geometry.Line2D>();
        int count = polygon.npoints;
        for (int i = 0; i < count; i++) {
            int j = (i + 1) % count;
            rescuecore2.misc.geometry.Point2D p1 = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[i], polygon.ypoints[i]);
            rescuecore2.misc.geometry.Point2D p2 = new rescuecore2.misc.geometry.Point2D(polygon.xpoints[j], polygon.ypoints[j]);
            rescuecore2.misc.geometry.Line2D line = new rescuecore2.misc.geometry.Line2D(p1, p2);
            lines.add(line);
        }
        return lines;
    }


    public static Set<rescuecore2.misc.geometry.Point2D> getIntersectionPoints(Polygon polygon, rescuecore2.misc.geometry.Line2D line) {
        Set<rescuecore2.misc.geometry.Point2D> point2Ds = new HashSet<>();
        List<rescuecore2.misc.geometry.Line2D> polyLines = Util.getLines(polygon);
        for (rescuecore2.misc.geometry.Line2D polyLine : polyLines) {
            rescuecore2.misc.geometry.Point2D intersect = getIntersection(line, polyLine);
            if (Util.contains(polyLine, intersect, 100)) {
                point2Ds.add(intersect);
            }
        }
        return point2Ds;
    }

    public static Pair<rescuecore2.misc.geometry.Point2D, rescuecore2.misc.geometry.Point2D> get2PointsAroundCenter(Edge entrance, rescuecore2.misc.geometry.Point2D center, int distance) {
        rescuecore2.misc.geometry.Line2D edgeLine = entrance.getLine();
        double slope = slope(edgeLine);
        int x1, y1, x2, y2;
        if (Double.isInfinite(slope)) {
            x1 = x2 = (int) center.getX();
            y1 = (int) (center.getY() + distance / 2);
            y2 = (int) (center.getY() - distance / 2);
        } else {
            double theta = Math.atan(slope);
            double sin = Math.sin(theta);
            double cos = Math.cos(theta);
            x1 = (int) (center.getX() + distance * cos / 2);
            y1 = (int) (center.getY() + distance * sin / 2);
            x2 = (int) (center.getX() - distance * cos / 2);
            y2 = (int) (center.getY() - distance * sin / 2);
        }
        return new Pair<>(new rescuecore2.misc.geometry.Point2D(x1, y1), new rescuecore2.misc.geometry.Point2D(x2, y2));
    }

    public static rescuecore2.misc.geometry.Point2D getPointInDistance(Line2D line, rescuecore2.misc.geometry.Point2D from, double distance) {//以from为圆点，distance为半径作圆，找到line直线上一个点
        rescuecore2.misc.geometry.Point2D point;
        double x1, y1;
        double deltaX = line.getX1() - line.getX2(), deltaY = line.getY1() - line.getY2();
        double slope = deltaY / deltaX;//斜率
        if (Double.isInfinite(slope)) {
            x1 = from.getX();
            y1 = from.getY() + Math.signum(deltaY) * distance;
        } else {
            double theta = Math.atan(slope);
            x1 = from.getX() - Math.signum(deltaX) * distance * Math.cos(theta);
            y1 = from.getY() - Math.signum(deltaX) * distance * Math.sin(theta);
        }
        point = new rescuecore2.misc.geometry.Point2D(x1, y1);
        return point;
    }

    public static rescuecore2.misc.geometry.Point2D getPoint(Pair<Integer, Integer> position) {
        return new rescuecore2.misc.geometry.Point2D(position.first(), position.second());
    }

    public static rescuecore2.misc.geometry.Line2D improveLine(rescuecore2.misc.geometry.Line2D line, double size) {
        double x0 = line.getOrigin().getX(), x1 = line.getEndPoint().getX();
        double y0 = line.getOrigin().getY(), y1 = line.getEndPoint().getY();
        double deltaY = y1 - y0;
        double deltaX = x1 - x0;
        double xF, yF;
        double slope;
        if (deltaX != 0)
            slope = deltaY / deltaX;
        else {
            if (deltaY > 0)
                slope = Double.MAX_VALUE;
            else
                slope = -Double.MAX_VALUE;
        }

        double theta = Math.atan(slope);
        double v = size * Math.abs(Math.cos(theta));
        if (deltaX > 0) {
            xF = x1 + v;
        } else {
            xF = x1 - v;
        }
        double v1 = size * Math.abs(Math.sin(theta));
        if (deltaY > 0) {
            yF = y1 + v1;
        } else {
            yF = y1 - v1;
        }
        rescuecore2.misc.geometry.Point2D endPoint = new rescuecore2.misc.geometry.Point2D(xF, yF);
        return new rescuecore2.misc.geometry.Line2D(line.getOrigin(), endPoint);
    }

    public static rescuecore2.misc.geometry.Line2D clipLine(rescuecore2.misc.geometry.Line2D line, double size) {
        double length = Util.lineLength(line);
        return improveLine(line, size - length);
    }

    public static Polygon clearAreaRectangle(double agentX, double agentY, double destinationX, double destinationY, double clearRad) {
        int clearLength = (int) Math.hypot(agentX - destinationX, agentY - destinationY);
        Vector2D agentToTarget = new Vector2D(destinationX - agentX, destinationY
                - agentY);

        if (agentToTarget.getLength() > clearLength)
            agentToTarget = agentToTarget.normalised().scale(clearLength);
        agentToTarget = agentToTarget.normalised().scale(agentToTarget.getLength() + 510);

        Vector2D backAgent = (new Vector2D(agentX, agentY))
                .add(agentToTarget.normalised().scale(-510));
        rescuecore2.misc.geometry.Line2D line = new rescuecore2.misc.geometry.Line2D(backAgent.getX(), backAgent.getY(),
                agentToTarget.getX(), agentToTarget.getY());

        Vector2D dir = agentToTarget.normalised().scale(clearRad);
        Vector2D perpend1 = new Vector2D(-dir.getY(), dir.getX());
        Vector2D perpend2 = new Vector2D(dir.getY(), -dir.getX());

        rescuecore2.misc.geometry.Point2D[] points = new rescuecore2.misc.geometry.Point2D[]{
                line.getOrigin().plus(perpend1),
                line.getEndPoint().plus(perpend1),
                line.getEndPoint().plus(perpend2),
                line.getOrigin().plus(perpend2)};
        int[] xPoints = new int[points.length];
        int[] yPoints = new int[points.length];
        for (int i = 0; i < points.length; i++) {
            xPoints[i] = (int) points[i].getX();
            yPoints[i] = (int) points[i].getY();
        }
        return new Polygon(xPoints, yPoints, points.length);
    }

    public static boolean equals(rescuecore2.misc.geometry.Line2D line1, rescuecore2.misc.geometry.Line2D line2) {
        return line1.getOrigin().getX() == line2.getOrigin().getX() && line1.getOrigin().getY() == line2.getOrigin().getY() && line1.getEndPoint().getX() == line2.getEndPoint().getX() && line1.getEndPoint().getY() == line2.getEndPoint().getY();
    }

    public static double distance(Polygon polygon, Point point, boolean igoreInside) {
        return distance(polygon, new rescuecore2.misc.geometry.Point2D(point.getX(), point.getY()), igoreInside);
    }

    public static double distance(Polygon polygon, Pair<Integer, Integer> location, boolean ignoreInside) {
        return distance(polygon, new rescuecore2.misc.geometry.Point2D(location.first(), location.second()), ignoreInside);
    }

    public static Pair<rescuecore2.misc.geometry.Line2D, rescuecore2.misc.geometry.Line2D> clearLengthLines(List<rescuecore2.misc.geometry.Point2D> polygon, double clearRad) {
        rescuecore2.misc.geometry.Line2D firstLine;
        rescuecore2.misc.geometry.Line2D secondLine;
        rescuecore2.misc.geometry.Line2D tempLine1 = new rescuecore2.misc.geometry.Line2D(polygon.get(0), polygon.get(1));
        double t1 = lineLength(tempLine1);
        if (Math.abs(t1 - clearRad) < 10) {
            firstLine = new rescuecore2.misc.geometry.Line2D(polygon.get(1), polygon.get(2));
            secondLine = new rescuecore2.misc.geometry.Line2D(polygon.get(0), polygon.get(3));
        } else {
            firstLine = tempLine1;
            secondLine = new rescuecore2.misc.geometry.Line2D(polygon.get(3), polygon.get(2));
        }

        return new Pair<>(firstLine, secondLine);  //To change body of created methods use File | Settings | File Templates.
    }

    public static Polygon clearAreaRectangle(rescuecore2.misc.geometry.Point2D startPoint, rescuecore2.misc.geometry.Point2D endPoint, int clearRadius) {
        return clearAreaRectangle(startPoint.getX(), startPoint.getY(), endPoint.getX(), endPoint.getY(), clearRadius);
    }

    public static Pair<rescuecore2.misc.geometry.Line2D, rescuecore2.misc.geometry.Line2D> clearLengthLines(Polygon polygon, int clearRadius) {
        return clearLengthLines(Util.getPoint2DList(polygon.xpoints, polygon.ypoints), clearRadius);
    }

    public static rescuecore2.misc.geometry.Line2D clipLine(rescuecore2.misc.geometry.Line2D line, double size, boolean fromCenter) {
        double length = lineLength(line);
        if (fromCenter) {
            double clipSize = (size - length) / 2;
            rescuecore2.misc.geometry.Line2D clip = improveLine(line, clipSize);
            return reverse(improveLine(reverse(clip), clipSize));
        } else {
            return improveLine(line, size - length);
        }
    }

    public static rescuecore2.misc.geometry.Line2D reverse(rescuecore2.misc.geometry.Line2D line) {
        rescuecore2.misc.geometry.Point2D end = line.getOrigin(), origin = line.getEndPoint();
        return new rescuecore2.misc.geometry.Line2D(origin, end);
    }

    public static Polygon scaleBySize3(Polygon polygon, double size) {
        Polygon result = new Polygon();
        rescuecore2.misc.geometry.Point2D center = new rescuecore2.misc.geometry.Point2D(polygon.getBounds().getCenterX(), polygon.getBounds().getCenterY());
        List<rescuecore2.misc.geometry.Line2D> polyLines = getLines(polygon);

        for (rescuecore2.misc.geometry.Line2D line2D : polyLines) {

            rescuecore2.misc.geometry.Point2D p1 = closestPoint(line2D, center);
            rescuecore2.misc.geometry.Line2D ln = new rescuecore2.misc.geometry.Line2D(center, p1);
            ln = improveLine(ln, size);
            rescuecore2.misc.geometry.Point2D p2 = ln.getEndPoint();
            double dx = p2.getX() - p1.getX();
            double dy = p2.getY() - p1.getY();

            rescuecore2.misc.geometry.Point2D origin = new rescuecore2.misc.geometry.Point2D(
                    line2D.getOrigin().getX() + dx,
                    line2D.getOrigin().getY() + dy
            );
            result.addPoint((int) origin.getX(), (int) origin.getY());

            rescuecore2.misc.geometry.Point2D end = new rescuecore2.misc.geometry.Point2D(
                    line2D.getEndPoint().getX() + dx,
                    line2D.getEndPoint().getY() + dy
            );
            result.addPoint((int) end.getX(), (int) end.getY());
        }
        return result;

    }


    public static Polygon retainPolygon(Polygon polygon1, Polygon polygon2) {
        return PolygonUtil.retainPolygon(polygon1,polygon2);
    }

    public static List<Point> getPointList(int[] apexList) {

        List<Point> points = new ArrayList<>();
        for (int i = 0; i < apexList.length; i += 2) {
            points.add(new Point(apexList[i], apexList[i + 1]));
        }

        return points;
    }

    public static ConvexHull convertToConvexHull(Polygon polygon) {
        com.vividsolutions.jts.algorithm.ConvexHull convexHull;
        Coordinate[] coordinates = new Coordinate[polygon.npoints];
        for (int i = 0; i < polygon.npoints; i++) {
            coordinates[i] = new Coordinate(polygon.xpoints[i], polygon.ypoints[i]);
        }
        convexHull = new ConvexHull(coordinates, new GeometryFactory());

        return convexHull;
    }

    public static EntityID getNearest(WorldInfo world, Collection<EntityID> locations, EntityID base) {
        EntityID result = null;
        double minDistance = Double.POSITIVE_INFINITY;
        for (EntityID next : locations) {
            double dist = world.getDistance(base, next);
            if (dist < minDistance) {
                result = next;
                minDistance = dist;
            }
        }
        return result;
    }
}
